close all; clear; clc;

d1 = 0.089159; d4 = 0.10915; d5 = 0.09465; d6 = 0.0823;
a3 = 0.425; a4 = 0.39225;

delta_alpha1 = 0.01; delta_alpha2 = 0.02; delta_alpha3 = 0.03; 
delta_alpha4 = -0.01; delta_alpha5 = -0.02; delta_alpha6 = 0; 

delta_a1 = 0.01; delta_a2 = 0.02; delta_a3 = 0.03;
delta_a4 = -0.01; delta_a5 = -0.02; delta_a6 = 0; 

delta_d1 = 0.01; delta_d2 = 0.02; delta_d3 = 0.03;
delta_d4 = -0.01; delta_d5 = -0.02; delta_d6 = -0.03;

delta_theta1 = 0.01; delta_theta2 = 0.02; delta_theta3 = 0.03; 
delta_theta4 = -0.01; delta_theta5 = -0.02; delta_theta6 = 0; 

Alpha = [0 + delta_alpha1, -pi/2 + delta_alpha2, 0 + delta_alpha3, 0 + delta_alpha4, pi/2 + delta_alpha5, -pi/2 + delta_alpha6];
A = [0 + delta_a1, 0 + delta_a2, a3 + delta_a3, a4 + delta_a4, 0 + delta_a5, 0 + delta_a6];
D = [d1 + delta_d1, 0 + delta_d2, 0 + delta_d3, d4 + delta_d4, d5 + delta_d5, d6 + delta_d6];
Theta = [0 + delta_theta1, -pi/2 + delta_theta2, 0 + delta_theta3, pi/2 + delta_theta4, 0 + delta_theta5, 0 + delta_theta6];

for i = 1:6
    L(i) = Link('alpha',Alpha(i),'a', A(i),'d', D(i));L(i).offset = Theta(i); L(i).qlim=[-pi, pi];L(i).mdh = 1;
end

robot = SerialLink(L, 'name', 'UR');

T = eye(4);
for i = 1:6
    T = T * transformation_mdh(Alpha(i), A(i), D(i), Theta(i), 1, 0);
    Slist(:, i) = [T(1:3, 3); cross(T(1:3, 4), T(1:3, 3))];
end

M = T;

T0 = transl([0.6, -0.2, 0.5]) * troty(pi) * trotz(pi);
T1 = transl([0.6, 0.2, 0.4]) * troty(pi) * trotz(pi);
T2 = transl([0.3, 0.0, 0.55]) * troty(pi) * trotz(pi);
T3 = T0;

num = 101;

Ti = ctraj(T0, T1, num);
Ti = cat(3, Ti(:, :, 1:end-1), ctraj(T1, T2, num));
Ti = cat(3, Ti(:, :, 1:end-1), ctraj(T2, T3, num));

t = (0:length(Ti) - 1) * 0.1;

q0 = [3; 0; -1; 0; 2; -2];
eomg = 0.001;
ev = 0.0001;
for i = 1:length(Ti) 
    [qe, success] = ikine(Slist, M, Ti(:, :, i), q0, eomg, ev);
    q0 = qe;
    qs(i, :) = qe';
    Te = robot.fkine(qe);
    px(i) = Te.t(1);
    py(i) = Te.t(2);
    pz(i) = Te.t(3);
end

figure(1);
plot3(px, py, pz);
robot.plot(qs);
